#include "actuator/param_srv/param_srv.h"

using namespace actuator;

void LoadYamlFile::Init(std::string &file_name, std::shared_ptr<boost::circular_buffer<st_par>> &param_buffer)
{
    YAML::Node param_yaml = YAML::LoadFile(file_name);
    actuator_params_ = param_yaml["actuator"];
    
    std::cout << "is define: " << actuator_params_["servo"].IsDefined() << std::endl;

    std::cout << "type:" << actuator_params_["servo"].Type() << std::endl;;
}

bool LoadYamlFile::GetParam(const std::string &name, float &value)
{
    if (actuator_params_[name].IsDefined())
    {
        value = actuator_params_[name].as<float>();
        return true;
    }
    return false;
}

bool LoadYamlFile::GetParam(const std::string &name, bool  &value)
{
    if (actuator_params_[name].IsDefined())
    {
        value = actuator_params_[name].as<bool>();
        return true;
    }
    return false;
}

bool LoadYamlFile::GetParam(const std::string &name, int   &value)
{
    if (actuator_params_[name].IsDefined())
    {
        value = actuator_params_[name].as<int>();
        return true;
    }
    return false;
}

bool LoadYamlFile::GetParam(const std::string &name, std::string &value)
{
    if (actuator_params_[name].IsDefined())
    {
        value = actuator_params_[name].as<std::string>();
        return true;
    }
    return false;
}

YAML::Node LoadYamlFile::GetYaml(const std::string &name)
{
    return actuator_params_[name];
}

void LoadVCS::Init(std::string &file_name, std::shared_ptr<boost::circular_buffer<st_par>> &param_buffer)
{

}

bool LoadVCS::GetParam(const std::string &name, float &value)
{

    return false;
}

bool LoadVCS::GetParam(const std::string &name, bool  &value)
{

    return false;
}

bool LoadVCS::GetParam(const std::string &name, int   &value)
{
    return false;
}

bool LoadVCS::GetParam(const std::string &name, std::string &value)
{
    return false;
}

void GetParamClient::Init(std::string &file_name, std::shared_ptr<boost::circular_buffer<st_par>> &param_buffer)
{
    std::shared_ptr<GetParams> y = std::make_shared<LoadYamlFile>();
    y->Init(file_name, param_buffer);
    std::shared_ptr<GetParams> v = std::make_shared<LoadVCS>();
    v->Init(file_name, param_buffer);

    ptr_params_.emplace_back(y);
    ptr_params_.emplace_back(v);
}

void GetParamClient::Append(const std::shared_ptr<GetParams>& s)
{
    ptr_params_.emplace_back(s);
}

void GetParamClient::Remove(const std::shared_ptr<GetParams>& s)
{
    // std::find(std::begin(ptr_params_), std::end(ptr_params_), s);
    const auto it = std::find(ptr_params_.begin(), ptr_params_.end(), s);
    if (it != ptr_params_.end())
    {
        ptr_params_.erase(it);
    }
}

bool GetParamClient::GetParam(const std::string &name, float &value)
{
    for (std::vector<std::shared_ptr<GetParams>>::iterator it=ptr_params_.begin();it!=ptr_params_.end();it++)
    {
        if ((*it)->GetParam(name, value))
        {
            return true;
        }
    }
    return false;
}

bool GetParamClient::GetParam(const std::string &name, bool  &value)
{
    for (std::vector<std::shared_ptr<GetParams>>::iterator it=ptr_params_.begin();it!=ptr_params_.end();it++)
    {
        if ((*it)->GetParam(name, value))
        {
            return true;
        }
    }
    return false;
}

bool GetParamClient::GetParam(const std::string &name, int   &value)
{
    for (std::vector<std::shared_ptr<GetParams>>::iterator it=ptr_params_.begin();it!=ptr_params_.end();it++)
    {
        if ((*it)->GetParam(name, value))
        {
            return true;
        }
    }
    return false;
}

bool GetParamClient::GetParam(const std::string &name, std::string &value)
{
    for (std::vector<std::shared_ptr<GetParams>>::iterator it=ptr_params_.begin();it!=ptr_params_.end();it++)
    {
        if ((*it)->GetParam(name, value))
        {
            return true;
        }
    }
    return false;
}


void GetParamClient::LoadAMRParams(const std::string &name, std::vector<SteerParams> &params)
{
    auto yaml = ptr_params_[0]->GetYaml(name);
    if (yaml.IsDefined())
    {
        SteerParams tmp;
        for (auto y: yaml)
        {
            tmp.pos_x = y["pos_x"].as<float>();
            tmp.pos_y = y["pos_y"].as<float>();

            params.emplace_back(tmp);
        }
    }
    printf("LoadAMRParams servo size: %ld\n", params.size());
}